#include "Filter.h"

Filter::Filter() {
}

Filter::~Filter() {
}

/** @param map is a 530 by 560 grid at CELL_RESOLUTION = 0.1 (usually)
 * */
bool Filter::validParticle(MyPose &pose,MATRIX &map){
	double mapX, mapY, mapTheta;
	int mapIndexX, mapIndexY;
	pose.getPoseInMap(mapX,mapY,mapTheta);

	PII pos = index(mapX,mapY,CELL_RESOLUTION);
	mapIndexX = pos.first;
	mapIndexY = pos.second;

	/*Pose has shot out of the map*/
	if(mapIndexX >= map.size() || mapIndexY >= map[0].size())
		return false;

	/*Pose is in visitable space or hit on a wall*/
	if(map[mapIndexX][mapIndexY] == SPACE || map[mapIndexX][mapIndexY] == WALL)
		return true;

	/*Pose is out the visitable area of the map*/
	return false;
}
